///////////////////////////////////////////////////////////////////////////////
//-----------------------------------------------------------------------------
#include <Scene/Culling/Frustum.h>
#include <Math/Vec3f.h>
#include <Scene/Culling/AABBox.h>
#include <Scene/Culling/Plane.h>
#include <math.h>

#define ANG2RAD 3.14159265358979323846/180.0

//-----------------------------------------------------------------------------

Frustum::Frustum()
{
}

//-----------------------------------------------------------------------------

Frustum::~Frustum()
{
}

//-----------------------------------------------------------------------------

void Frustum::setCamInternals(f32 angle, f32 ratio, f32 nearD, f32 farD)
{
	this->ratio = ratio;
	this->angle = angle;
	this->nearD = nearD;
	this->farD = farD;

	tang = (f32)tan(angle* ANG2RAD * 0.5) ;
	nh = nearD * tang;
	nw = nh * ratio; 
	fh = farD  * tang;
	fw = fh * ratio;
}

//-----------------------------------------------------------------------------

void Frustum::setCamDef( const Vec3f &p, const Vec3f &l, const Vec3f &u)
{
	Vec3f dir,nc,fc,X,Y,Z;

	Z = p - l;
	Z.normalize();

	X = u ^ Z;
	X.normalize();

	Y = Z ^ X;

	nc = p - Z * nearD;
	fc = p - Z * farD;

#if defined(DEBUG)

	ntl = nc + Y * nh - X * nw;
	ntr = nc + Y * nh + X * nw;
	nbl = nc - Y * nh - X * nw;
	nbr = nc - Y * nh + X * nw;

	ftl = fc + Y * fh - X * fw;
	ftr = fc + Y * fh + X * fw;
	fbl = fc - Y * fh - X * fw;
	fbr = fc - Y * fh + X * fw;

	pl[TOP].set3Points(ntr,ntl,ftl);
	pl[BOTTOM].set3Points(nbl,nbr,fbr);
	pl[LEFT].set3Points(ntl,nbl,fbl);
	pl[RIGHT].set3Points(nbr,ntr,fbr);
	pl[NEARP].set3Points(ntl,ntr,nbr);
	pl[FARP].set3Points(ftr,ftl,fbl);

#else

	Vec3f aux,normal;

	aux = -Z;
	pl[NEARP].setNormalAndPoint(aux,nc);
	pl[FARP].setNormalAndPoint(Z,fc);

	aux = (nc + Y*nh) - p;
	aux.normalize();
	normal = aux ^ X;
	aux = nc+Y*nh;
	pl[TOP].setNormalAndPoint(normal,aux);

	aux = (nc - Y*nh) - p;
	aux.normalize();
	normal = X ^ aux;
	aux = nc-Y*nh;
	pl[BOTTOM].setNormalAndPoint(normal,aux);

	aux = (nc - X*nw) - p;
	aux.normalize();
	normal = aux ^ Y;
	aux = nc-X*nw;
	pl[LEFT].setNormalAndPoint(normal,aux);

	aux = (nc + X*nw) - p;
	aux.normalize();
	normal = Y ^ aux;
	aux = nc+X*nw;
	pl[RIGHT].setNormalAndPoint(normal,aux);

#endif
}

//-----------------------------------------------------------------------------

int Frustum::isPointInFrustum(const Vec3f &p)
{
	int result = INSIDE;
	for(int i=0; i < 6; i++) {

		if (pl[i].distance(p) < 0)
			return OUTSIDE;
	}
	return(result);
}

//-----------------------------------------------------------------------------

int Frustum::isSphereInFrustum(const Vec3f &p, f32 raio)
{
	int result = INSIDE;
	f32 distance;

	for(int i=0; i < 6; i++) {
		distance = pl[i].distance(p);
		if (distance < -raio)
			return OUTSIDE;
		else if (distance < raio)
			result =  INTERSECT;
	}
	return(result);
}

//-----------------------------------------------------------------------------

int Frustum::isAABBoxInFrustum(AABBox &b)
{
	int result = INSIDE;
	for(int i=0; i < 6; i++) {

		if (pl[i].distance(b.getVertexP(pl[i].normal)) < 0)
			return OUTSIDE;
		else if (pl[i].distance(b.getVertexN(pl[i].normal)) < 0)
			result =  INTERSECT;
	}
	return(result);
 }

#if	defined(DEBUG)
//-----------------------------------------------------------------------------

void Frustum::drawPoints()
{
	glBegin(GL_POINTS);

		glVertex3f(ntl.x,ntl.y,ntl.z);
		glVertex3f(ntr.x,ntr.y,ntr.z);
		glVertex3f(nbl.x,nbl.y,nbl.z);
		glVertex3f(nbr.x,nbr.y,nbr.z);

		glVertex3f(ftl.x,ftl.y,ftl.z);
		glVertex3f(ftr.x,ftr.y,ftr.z);
		glVertex3f(fbl.x,fbl.y,fbl.z);
		glVertex3f(fbr.x,fbr.y,fbr.z);

	glEnd();
	GL_CHECK_ERRORS();
}

//-----------------------------------------------------------------------------

void Frustum::drawLines()
{
	glBegin(GL_LINE_LOOP);
	//near plane
		glVertex3f(ntl.x,ntl.y,ntl.z);
		glVertex3f(ntr.x,ntr.y,ntr.z);
		glVertex3f(nbr.x,nbr.y,nbr.z);
		glVertex3f(nbl.x,nbl.y,nbl.z);
	glEnd();

	glBegin(GL_LINE_LOOP);
	//far plane
		glVertex3f(ftr.x,ftr.y,ftr.z);
		glVertex3f(ftl.x,ftl.y,ftl.z);
		glVertex3f(fbl.x,fbl.y,fbl.z);
		glVertex3f(fbr.x,fbr.y,fbr.z);
	glEnd();

	glBegin(GL_LINE_LOOP);
	//bottom plane
		glVertex3f(nbl.x,nbl.y,nbl.z);
		glVertex3f(nbr.x,nbr.y,nbr.z);
		glVertex3f(fbr.x,fbr.y,fbr.z);
		glVertex3f(fbl.x,fbl.y,fbl.z);
	glEnd();

	glBegin(GL_LINE_LOOP);
	//top plane
		glVertex3f(ntr.x,ntr.y,ntr.z);
		glVertex3f(ntl.x,ntl.y,ntl.z);
		glVertex3f(ftl.x,ftl.y,ftl.z);
		glVertex3f(ftr.x,ftr.y,ftr.z);
	glEnd();

	glBegin(GL_LINE_LOOP);
	//left plane
		glVertex3f(ntl.x,ntl.y,ntl.z);
		glVertex3f(nbl.x,nbl.y,nbl.z);
		glVertex3f(fbl.x,fbl.y,fbl.z);
		glVertex3f(ftl.x,ftl.y,ftl.z);
	glEnd();

	glBegin(GL_LINE_LOOP);
	// right plane
		glVertex3f(nbr.x,nbr.y,nbr.z);
		glVertex3f(ntr.x,ntr.y,ntr.z);
		glVertex3f(ftr.x,ftr.y,ftr.z);
		glVertex3f(fbr.x,fbr.y,fbr.z);
	glEnd();
	GL_CHECK_ERRORS();
}

//-----------------------------------------------------------------------------

void Frustum::drawPlanes()
{
	glBegin(GL_QUADS);

	////near plane
	//	glVertex3f(ntl.x,ntl.y,ntl.z);
	//	glVertex3f(ntr.x,ntr.y,ntr.z);
	//	glVertex3f(nbr.x,nbr.y,nbr.z);
	//	glVertex3f(nbl.x,nbl.y,nbl.z);

	////far plane
	//	glVertex3f(ftr.x,ftr.y,ftr.z);
	//	glVertex3f(ftl.x,ftl.y,ftl.z);
	//	glVertex3f(fbl.x,fbl.y,fbl.z);
	//	glVertex3f(fbr.x,fbr.y,fbr.z);

	//bottom plane
		glVertex3f(nbl.x,nbl.y,nbl.z);
		glVertex3f(nbr.x,nbr.y,nbr.z);
		glVertex3f(fbr.x,fbr.y,fbr.z);
		glVertex3f(fbl.x,fbl.y,fbl.z);

	//top plane
		glVertex3f(ntr.x,ntr.y,ntr.z);
		glVertex3f(ntl.x,ntl.y,ntl.z);
		glVertex3f(ftl.x,ftl.y,ftl.z);
		glVertex3f(ftr.x,ftr.y,ftr.z);

	//left plane

		glVertex3f(ntl.x,ntl.y,ntl.z);
		glVertex3f(nbl.x,nbl.y,nbl.z);
		glVertex3f(fbl.x,fbl.y,fbl.z);
		glVertex3f(ftl.x,ftl.y,ftl.z);

	// right plane
		glVertex3f(nbr.x,nbr.y,nbr.z);
		glVertex3f(ntr.x,ntr.y,ntr.z);
		glVertex3f(ftr.x,ftr.y,ftr.z);
		glVertex3f(fbr.x,fbr.y,fbr.z);

	glEnd();
	GL_CHECK_ERRORS();
}

//-----------------------------------------------------------------------------

void Frustum::drawNormals()
{
	Vec3f a,b;

	glBegin(GL_LINES);

		// near
		a = (ntr + ntl + nbr + nbl) * 0.25;
		b = a + pl[NEARP].normal;
		glVertex3f(a.x,a.y,a.z);
		glVertex3f(b.x,b.y,b.z);

		// far
		a = (ftr + ftl + fbr + fbl) * 0.25;
		b = a + pl[FARP].normal;
		glVertex3f(a.x,a.y,a.z);
		glVertex3f(b.x,b.y,b.z);

		// left
		a = (ftl + fbl + nbl + ntl) * 0.25;
		b = a + pl[LEFT].normal;
		glVertex3f(a.x,a.y,a.z);
		glVertex3f(b.x,b.y,b.z);
		
		// right
		a = (ftr + nbr + fbr + ntr) * 0.25;
		b = a + pl[RIGHT].normal;
		glVertex3f(a.x,a.y,a.z);
		glVertex3f(b.x,b.y,b.z);
		
		// top
		a = (ftr + ftl + ntr + ntl) * 0.25;
		b = a + pl[TOP].normal;
		glVertex3f(a.x,a.y,a.z);
		glVertex3f(b.x,b.y,b.z);
		
		// bottom
		a = (fbr + fbl + nbr + nbl) * 0.25;
		b = a + pl[BOTTOM].normal;
		glVertex3f(a.x,a.y,a.z);
		glVertex3f(b.x,b.y,b.z);

	glEnd();
	GL_CHECK_ERRORS();
}
#endif

//-----------------------------------------------------------------------------
///////////////////////////////////////////////////////////////////////////////
